% outputs from humanData
% get average mass
% add path
addpath('/home/shu/Documents/Research_Related/Data/NewFlatGround/')
addpath('~/Documents/Research_Related/COM_HD/')
addpath('~/Documents/Research_Related/COM_HD/human_data_analysis/')
addpath('~/Documents/Research_Related/COM_HD/human_data_analysis/humanData')
addpath('/home/shu/Documents/test/K2_NTorso_HM')
addpath('./Func/')
addpath('./PlotFunc/') % plot functions
name  = {'Ayushi','Charles','Fred','Martin','Ram','Selina',...
    'lily','po','ryan','vic'};


time_rec = zeros(9,1);


for i = 2:length(name)
    
% get human data, with breaking pts for each step
humanData = getData(name{1,i});
% sum_weight  = sum_weight+humanData.weight; % for average weight
% breaking pts for each step
a = humanData.sns_brk(1);
b = humanData.sns_brk(2);
c = humanData.sns_brk(3);
d = humanData.sns_brk(4);

% position
if i <= 6 % old human data
    [xpos,ypos,zpos] = getPosCartesian(humanData);
else 
    [xpos,ypos,zpos] = getPosCartesianNew(humanData); % new human data
end

%%%%%%%%%%%  time  %%%%%%%%%%%%%%
% scale time
% from [c:d]
ns_time_pre = (0:(length(b:c)-1))'/480;
ns_ret = ns_time_pre;

xpara_time = 1/ns_ret(end); % simply sacled time
ns_time = ns_ret*xpara_time;


%%%% record time %%%%%%%%
time_rec(i-1) = ns_ret(end);

%%%%%%%%%%%% OUTPUT %%%%%%%%%%%%%%%%%
if i==7 || i==8 || i==10 
    % right leg is the swing leg
    angle = Knee2D_angle(xpos,ypos,zpos,humanData.sns_brk,2);
else
    % left leg is the swing leg
    angle = Knee2D_angle(xpos,ypos,zpos,humanData.sns_brk,1);
end

%%%%%%%%%%%% 1 stance knee  %%%%%%%%%%%%%%%%
[ave_dataSK,ave_timeSK] = BeamData(ns_time,angle.sknee);
ave_dataSK_s(i-1,:) = ave_dataSK;
% plot(ns_ret,angle.sknee)
% plot(ave_timeSK,ave_dataSK);hold on;

%%%%%%%%%%%% 2 non-stance knee %%%%%%%%%%%%
[ave_dataNSK,ave_timeNSK] = BeamData(ns_time,angle.nsknee);
ave_dataNSK_s(i-1,:) = ave_dataNSK;
% plot(ave_timeNSK, ave_dataNSK,'g');hold on;

%%%%%%%%%%%% 3 Stance Ankle Angle %%%%%%%%%%%%
[ave_dataSA,ave_timeSA] = BeamData(ns_time, angle.sa);
ave_dataSA_s(i-1,:) =ave_dataSA;
% plot(ave_timeSA,ave_dataSA,'g');hold on;
 
%%%%%%%%%%%% 8 hip angle %%%%%%%%%%%%%
[ave_dataHip,ave_timeHip] = BeamData(ns_time,angle.hip);
% plot(ns_ret,ret_torso)
ave_dataHip_s(i-1,:) =ave_dataHip;
% plot(ave_timeHip,ave_dataHip,'g');hold on; % test

end


ave_time = mean(time_rec);



% ns_time = mean(time_rec)*ave_timeSK';
%%
load('4q.mat')
ns_time = q(1,:)/q(1,end);
% 1. stance knee
[x_meanSK,upperBSK,lowerBSK]=meanValue(ave_dataSK_s);
meanValuePlot_robotConfig(ave_timeSK,x_meanSK,...
    '4ConfigSknee.eps','Angle(rad)',q(3,:),ns_time,6)

% 2. nonstance knee
[x_meanNSK,upperBNSK,lowerBNSK]=meanValue(ave_dataNSK_s);
meanValuePlot_robotConfig(ave_timeSK,x_meanNSK,...
    '4ConfigNsknee.eps','Angle(rad)',q(5,:),ns_time,7)

% 3. hip
[x_meanHip,upperBHip,lowerBHip]=meanValue(ave_dataHip_s);
meanValuePlot_robotConfig(ave_timeHip,x_meanHip,...
    '4ConfigHip.eps','Angle(rad)',-q(4,:),ns_time,5)

% 4. stance ankle angle
[x_meanSA,upperBSA,lowerBSA]=meanValue(ave_dataSA_s);
meanValuePlot_robotConfig(ave_timeSA,x_meanSA,...
    '4ConfigSA.eps','Angle(rad)',q(2,:),ns_time,8)

